/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
/**
 * \file
 * \brief Implementation for explicit multicast.
 * \author Antti Maula <antti.maula@tkk.fi>
 */
#include "fastProtocol.h"
#include "fastProtocolExplicitMulticast.h"
#include "fastProtocol_util.h"
#include "owndebug.h"
#include "ownutils.h"
#include "../connectionList.h"
#include "../connectionListEntry.h"
#include "idmap.h"
#include <assert.h>
#include <inttypes.h>

//*************************************************************************
//*************************************************************************
//*************************************************************************
//*************************************************************************

static int explicitMulticastSendToHubs(TFastProtocol *p,
                                       TConnectionList *l, 
                                       TConnectionListEntry *e,
                                       const TFastProtocolHeader *fastheader,
                                       const uint8_t *payload,
                                       const unsigned int payloadsize,
                                       const unsigned int *targetIdArrayPtr,
                                       const int targetIdArrayCount)
{
  const int localhubid = connectionListGetLocalHubID(l) >> 16; // For fast ref.
  int firstOwnhub = -1;
  int lastHubTarget = 0;
  int r,i;

  // A few simple sanity checks
  assert(targetIdArrayCount > 0);
  assert(targetIdArrayPtr != NULL);

  // idmaps for GWs
  TIDMap *gwmap = idMapCreate();
  assert(gwmap != NULL);
  // Create a idMap with maximum of 20 bins (Larger AP counts are
  // slow anyway, so this is a very good tradeoff between memory and
  // performance)
  r = idMapInit(gwmap, MIN( 20, targetIdArrayCount ));
  assert(!r);
  
  // Now process all targets ;;; Extract Gateways and insert to list
  for(i=0; i < targetIdArrayCount; ++i) {
    const int hubid = ID2HUBID(targetIdArrayPtr[i]);  
    
    if (hubid == localhubid) {
      //dPrint(ODTEST,"Local APID, skip. (%04x)", hubid);
      if (firstOwnhub == -1) {
        firstOwnhub = i;
        //dPrint(ODTEST,"First LocalAPID found at index %u, entry %08x (local AP %04x)",
        //       i, targetIdArrayPtr[i], hubid);
      }

    } else if (hubid == 0) {
      //dPrint(ODTEST,"Zeroed out APID, skip.");

    } else if (hubid == lastHubTarget) {
      //dPrint(ODTEST,"HubId matches lastAPTarget (%04x), skip.", hubid);

    } else if (hubid == CONNECTIONLIST_HUBID_BROADCAST) {
      dPrint(ODERROR,"What!? BROADCAST id? No way.");

    } else {
      unsigned int gw_id = 0;
      const int cr = connectionListGetGatewayIDbyTargetID(l, targetIdArrayPtr[i], &gw_id);
      if (cr != CONNECTIONLIST_OK) {
        dPrint(ODTEST,"Unrouteable GW %08x : Skip", targetIdArrayPtr[i]);
        continue; // Jump to beginning of for()

      } else {
        //dPrint(ODTEST,"Got GW for %08x --> %08x", targetIdArrayPtr[i], gw_id);
        const TIDMapEntry *me = idMapGetEntry(gwmap, gw_id);
        if (!me) {
          // No such GW, add, and add a new idmap inside it for storing the allowed target addresses
          TIDMap *idmap = idMapCreate();
          assert(idmap != NULL);
          r = idMapInit(idmap, MIN(20, targetIdArrayCount));
          assert(!r);

          //dPrint(ODTEST,"GW %08x does not have entry in the top list, add it!", gw_id);

          // Insert target (NOTE; ONLY KEY USED!)
          r = idMapAddEntry(idmap, targetIdArrayPtr[i], NULL);
          assert(!r);

          // Insert to GW list
          r = idMapAddEntry(gwmap, gw_id, (void*)idmap);
          assert(!r);
        
        } else {
          //dPrint(ODTEST,"GW %08x found from top list, just add the target ID", gw_id);
          // GW Found.
          // Insert target (NOTE; ONLY KEY USED!)
          r = idMapAddEntry((TIDMap *)me->entry, targetIdArrayPtr[i], NULL);
          assert(!r);
          
        }
        
      }
    }
  }
  //dPrint(ODTEST,"Gateways processed -> Construct final receiver lists and transmit");

  // Allocate memory for the per-GW targetIDarray and create easier pointers
  uint8_t *multicastHeader = malloc( sizeof(TFastProtocolHeader) +
                                     (sizeof(uint32_t) * targetIdArrayCount) );
  TFastProtocolHeader *outhdr = (TFastProtocolHeader *)multicastHeader;
  uint32_t *outtargets = (uint32_t *)(multicastHeader + sizeof(TFastProtocolHeader));
  assert(multicastHeader);
  
  // Then iterate over all found distinct gateways
  TIDMapEntryIterator iter;
  idMapGetNextEntryIterator(gwmap, &iter);
  const TIDMapEntry *ei;
  while((ei = idMapGetNextEntry(gwmap, &iter))) {
    //dPrint(ODTEST,"Now processing entries destined to GW %08x", ei->nodeID);
    TIDMap *idmap = ei->entry;
    TIDMapEntryIterator siter;
    idMapGetNextEntryIterator(idmap, &siter);
    const TIDMapEntry *sei;

    // Now, clear the localTargetIdArray before starting again
    memset(outtargets, 0x00, sizeof(uint32_t) * targetIdArrayCount);
    unsigned int localTargetIdCounter = 0;
    while((sei = idMapGetNextEntry(idmap, &siter))) {
      //dPrint(ODTEST,"Subitem: %08x, %p", sei->nodeID, sei->entry);
      outtargets[localTargetIdCounter] = sei->nodeID;
      localTargetIdCounter++;
    }

    /////////// Recipient list contructed!
    // Construct headers and send!
    // Set FastProtocolHeader
    *outhdr = *fastheader;
    outhdr->target_count = localTargetIdCounter;
    outhdr->protocol_id.total_size = ( payloadsize + 
                                       sizeof(TFastProtocolHeader) + 
                                       (sizeof(uint32_t) * localTargetIdCounter) );
    
    // Get target for the gateway
    TConnectionListEntry *gw_target = connectionListGetConnectionEntryByID(l, ei->nodeID);

    if (gw_target) {
      connectionListSendRawDataByEntry(l,
                                       (const char *)multicastHeader,
                                       sizeof(TFastProtocolHeader) + (sizeof(uint32_t) * localTargetIdCounter),
                                       gw_target );
      connectionListSendRawDataByEntry(l,
                                       (const char *)payload,
                                       payloadsize,
                                       gw_target );
    } else {
      dPrint(ODTEST,"Could not find an ConnectionListEntry for gw_target %08x!", ei->nodeID);

    }
    
    // AFTER sending the message; free memory allocated for the just-processed list
    idMapDeinit(idmap);
    idMapDestroy(idmap);
  }

  // ALL processed, destroy the top-level IDmap
  idMapDeinit(gwmap);
  idMapDestroy(gwmap);

  // Clear the allocated multicastHeader
  free(multicastHeader);

  // return
  return firstOwnhub;
}
//*************************************************************************

static int explicitMulticastSendToLocalClients(TFastProtocol *p,
                                               TConnectionList *l, 
                                               TConnectionListEntry *e,
                                               TFastProtocolHeader *fastheader,
                                               const unsigned char *payloadPtr,
                                               const unsigned int *targetIdArrayPtr,
                                               const int targetIdArrayCount,
                                               const int firstOwnhub)
{
  int r,i;
  int result = 0;
  int cansend = 1;
  const int localhubid = ID2HUBID(connectionListGetLocalHubID(l)); // For fast ref.

  assert(firstOwnhub >= 0);
  
  // Now, send the result to all local targets!
  for(i = firstOwnhub; i < targetIdArrayCount; i++) {
    // Take the address to temporary variable
    int hubid = ID2HUBID(targetIdArrayPtr[i]);
    int clientid = ID2CLIENTID(targetIdArrayPtr[i]);
    
    if (hubid == localhubid) {
      //      dPrint(ODTEST, "Multicast: Sending to local client: '%04x:%04x'", hubid, clientid);
      
      // Modify fastheader target to make the packet look 'unicast'
      fastheader->target_id = HUBCLIENT2ID(hubid, clientid); 
      
      // Check that the clientid is not broadcast ID or LOCAL id (0000, FFFF)
      if (clientid == CONNECTIONLIST_CLIENTID_BROADCAST ||
          clientid == CONNECTIONLIST_CLIENTID_LOCAL) {
        cansend = 0;
      }

      // Finally, if the cansend is still OK to go, kah :
      if (cansend) {
        //dPrint(ODTEST, "Sending to (target '%08x', %d bytes)",
        //       targetIdArrayPtr[i], fastheader->protocol_id.total_size );
        // Send the data through connectionList
        r = connectionListSendDataByID(l, 
                                       &e->inputBuffer, 
                                       0, // Start from zero - send all!
                                       fastheader->protocol_id.total_size,
                                       fastheader->target_id);
        // Process the connectionList Reply (may send reply!), notice the use of 'r'
        r = handleConnectionListReply(p, l, e, fastheader, r);
        
      } else {
        dPrint(8, "No permission to send, skipping target '%08x'", 
               targetIdArrayPtr[i]);
	
      }
    } else {
      // No more local targets (assuming sorted list)
      //dPrint(8, "Finished sending to local targets.");
      break;
      
    }
  }

  return result;
}
//*************************************************************************

#ifdef ENABLE_EXPLICIT_MULTICAST_ENTRYLIST_QSORT

static int cmphubid(const void *p1, const void *p2)
{
  return *(int*)p1 < *(int*)p2;
}
#endif //ENABLE_EXPLICIT_MULTICAST_ENTRYLIST_QSORT
//*************************************************************************

EProtocolExit dispatchExplicitMulticastPacket(TFastProtocol *p,
                                              TConnectionList *l, 
                                              TConnectionListEntry *e,
                                              TFastProtocolHeader *fastheader)
{
  int r = protocolExitNoError;
  unsigned int *targetIdArrayPtr;
  unsigned int *targetIdArrayCopyPtr;
  int targetIdCount;
  const unsigned char *payloadPtr;
  int nonpayloadsize;
  int payloadsize;
  
  // Verify that top level dispatcher did it's job properly!
  assert( fastheader->flags & FASTPROTOCOL_FLAG_MODE_EXPLICIT_MULTICAST );

  // Store the targetIDCount in local variable.
  targetIdCount = fastheader->target_count;

  // Check
  if (targetIdCount <= 0) {
    dPrint(ODERROR,"targetIdCount <= 0, invalid Multicast frame received!");
    return protocolExitError;
  }

  // Calculate the address of the payload itself & parameters
  targetIdArrayPtr = (unsigned int *)(bufferGetPtr(&e->inputBuffer) + 
                                      sizeof(TFastProtocolHeader));
  payloadPtr = (const unsigned char *)(targetIdArrayPtr+targetIdCount);
  nonpayloadsize = ( sizeof(TFastProtocolHeader) +
                     (targetIdCount*sizeof(unsigned int)) );
  payloadsize = ( fastheader->protocol_id.total_size - nonpayloadsize );
  
  // Now, test that the packet & header sizes match!
  if (payloadsize < 0) {
    dPrint(ODERROR, "Invalid Explicit multicast packet received! (From: '%08x')",
           e->connectionID);
    r = -2; // Real error!
    return r; // FIXME: return in middle.
  }


  // Sort the Array! (NOTE: This is quite expensive - so for testing only!)
#ifdef ENABLE_EXPLICIT_MULTICAST_ENTRYLIST_QSORT
  qsort((void*)targetIdArrayPtr, targetIdCount, 
        sizeof(const unsigned int), cmphubid);
#endif //ENABLE_EXPLICIT_MULTICAST_ENTRYLIST_QSORT

  
#warning FIXME: Why is this copied here?
  // Calculate the address of ID Array & Copy it (Dynamic allocation)
  targetIdArrayCopyPtr = malloc(sizeof(uint32_t) * targetIdCount);
  assert(targetIdArrayCopyPtr);
  memcpy(targetIdArrayCopyPtr,
         targetIdArrayPtr,
         (targetIdCount * sizeof(unsigned int)));
  
  // Then, Send to hubs!
  r = explicitMulticastSendToHubs(p, l, e,
                                  fastheader, payloadPtr, payloadsize, 
                                  targetIdArrayPtr, targetIdCount);

  // And if the hubs sender found a 'local' entry key, send to those too
  if (r >= 0) {

    // Modify the incoming data to 'look like' normal frame
    // a) Move the payload back
    // b) modify the 'total frame size' (doesn't interfere with packet shift
    //    on upper level, because the size is stored there locally.
    memmove((void*)targetIdArrayPtr, payloadPtr, payloadsize); // Move the payload 'back'
    fastheader->protocol_id.total_size -= (targetIdCount*sizeof(unsigned int)); 
    fastheader->flags &= ~FASTPROTOCOL_FLAG_MODE_EXPLICIT_MULTICAST; // Clear MULTICAST bit 
    
    // Now, the payload really starts at the beginning of the old targetIdArray
    payloadPtr = (const unsigned char *)targetIdArrayPtr;


    // Send to clients
    r = explicitMulticastSendToLocalClients(p, l, e,
                                            fastheader,
                                            payloadPtr,
                                            targetIdArrayCopyPtr, targetIdCount,
                                            r);
    assert(r == 0); // FIXME?
    r = protocolExitNoError;

  } else {
    
    r = protocolExitNoError;
  }

  free(targetIdArrayCopyPtr);


  return r;
}
//*************************************************************************
//*************************************************************************
//*************************************************************************
//*************************************************************************
//*************************************************************************
